using System;
using UnityEngine;

public static class CCMathHelper
{
    public static Vector3 ToCCEuler(Quaternion q, bool outerZ = false)
    {

        var x = q.x; var y = q.y; var z = q.z; var w = q.w;
        double bank = 0;
        double heading = 0;
        double attitude = 0;
        double test = x * y + z * w;
        if (test > 0.499999)
        {
            bank = 0; // default to zero
            heading = toDegree(2 * Math.Atan2(x, w));
            attitude = 90;
        }
        else if (test < -0.499999)
        {
            bank = 0; // default to zero
            heading = -toDegree(2 * Math.Atan2(x, w));
            attitude = -90;
        }
        else
        {
            var sqx = x * x;
            var sqy = y * y;
            var sqz = z * z;
            bank = toDegree(Math.Atan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz));
            heading = toDegree(Math.Atan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz));
            attitude = toDegree(Math.Asin(2 * test));
            if (outerZ)
            {
                bank = -180 * Math.Sign(bank + 1e-6) + bank;
                heading = -180 * Math.Sign(heading + 1e-6) + heading;
                attitude = 180 * Math.Sign(attitude + 1e-6) - attitude;
            }
        }
        Vector3 outer = new Vector3();
        outer.x = (float)bank; outer.y = (float)heading; outer.z = (float)attitude;
        return outer;
    }
    private static float toDegree(double a)
    {
        return (float)(a * 180.0 / Math.PI);
    }
}
